/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-10-18 16:54:35
 * @LastEditTime: 2021-10-27 14:16:13
 * @LastEditors: Sang Hao
 */
#include <Eigen/Dense>

#include "lidar_slam/publisher/loop_pose_publisher.hpp"

namespace lidar_slam {
/**
 * @description: 构造函数
 * @param  {*}
 * @return {*}
 */
LoopPosePublisher::LoopPosePublisher(ros::NodeHandle& nh, std::string topic_name, 
	std::string frame_id, int buff_size) 
	:nh_(nh), frame_id_(frame_id) {
	publisher_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic_name, buff_size);
}
/**
 * @description: 将loop_pose整合到geometry_msgs::PoseWithCovarianceStamped中发布出去
 * @param  {*}
 * @return {*}
 */
void LoopPosePublisher::Publish(LoopPose& loop_pose) {
	geometry_msgs::PoseWithCovarianceStamped pose_stamped;

	ros::Time ros_time(float(loop_pose.time));
	pose_stamped.header.stamp = ros_time;
	pose_stamped.header.frame_id = frame_id_;
	pose_stamped.pose.pose.position.x = loop_pose.pose(0, 3);
	pose_stamped.pose.pose.position.y = loop_pose.pose(1, 3);
	pose_stamped.pose.pose.position.z = loop_pose.pose(2, 3);

	Eigen::Quaternionf q = loop_pose.GetQuaternion();

	pose_stamped.pose.pose.orientation.x = q.x();
	pose_stamped.pose.pose.orientation.y = q.y();	
	pose_stamped.pose.pose.orientation.z = q.z();
	pose_stamped.pose.pose.orientation.w = q.w();

	// 没有地方存了，因此存储到方差里了 // TODO:可以尝试自定义消息类型
	pose_stamped.pose.covariance[0] = (double)loop_pose.index0;
	pose_stamped.pose.covariance[1] = (double)loop_pose.index1;

	publisher_.publish(pose_stamped);
}
/**
 * @description: 判断是否有订阅函数 
 * @param  {*}
 * @return {*}
 */
bool LoopPosePublisher::LoopPosePublisher::HasSubscribers() {
	return publisher_.getNumSubscribers() != 0;
}
}